using System;
using System.Collections;
using System.Runtime.CompilerServices;
using System.Threading;
using LightCAD.MathLib;

namespace LightCAD.MathLib
{
    public abstract class Vector
    {
        public abstract bool Equals(Vector obj);
    }
    public partial class Vector2 : Vector, IIOArray
    {
        public static int newCount = 0;
        #region Properties

        public double X;
        public double Y;

        #endregion
        public Vector2() : this(0, 0)
        { }
        public Vector2(double x = 0, double y = 0)
        {
            Interlocked.Add(ref newCount, 1);
            X = x;
            Y = y;
        }
        public double this[int i]
        {
            get
            {
                return i == 0 ? this.X : i == 1 ? this.Y : throw new Exception("error index");
            }
            set
            {
                if (i == 0) this.X = value;
                else if (i == 1) this.Y = value;
                else throw new Exception("error index");
            }
        }
        public double Width
        {
            [MethodImpl((MethodImplOptions)768)]
            get
            {
                return X;
            }
            [MethodImpl((MethodImplOptions)768)]
            set
            {
                X = value;
            }
        }
        public double Height
        {
            [MethodImpl((MethodImplOptions)768)]
            get
            {
                return Y;
            }
            [MethodImpl((MethodImplOptions)768)]
            set
            {
                Y = value;
            }
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 Set(double x, double y)
        {
            X = x;
            Y = y;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 SetScalar(double scalar)
        {
            X = scalar;
            Y = scalar;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 SetX(double x)
        {
            X = x;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 SetY(double y)
        {
            Y = y;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 SetComponent(double index, double value)
        {
            switch (index)
            {
                case 0: X = value; break;
                case 1: Y = value; break;
                default: throw new Error("index is out of range: " + index);
            }
            return this;
        }
        public double GetComponent(double index)
        {
            switch (index)
            {
                case 0: return X;
                case 1: return Y;
                default: throw new Error("index is out of range: " + index);
            }
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 Clone()
        {
            return new Vector2(X, Y);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 Copy(Vector2 v)
        {
            X = v.X;
            Y = v.Y;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 Add(Vector2 v)
        {
            X += v.X;
            Y += v.Y;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 Abs(Vector2 v)
        {
            X = Math.Abs(v.X);
            Y = Math.Abs(v.Y);
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 AddScalar(double s)
        {
            X += s;
            Y += s;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 AddVectors(Vector2 a, Vector2 b)
        {
            X = a.X + b.X;
            Y = a.Y + b.Y;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 AddScaledVector(Vector2 v, double s)
        {
            X += v.X * s;
            Y += v.Y * s;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 Sub(Vector2 v)
        {
            X -= v.X;
            Y -= v.Y;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 SubScalar(double s)
        {
            X -= s;
            Y -= s;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 SubVectors(Vector2 a, Vector2 b)
        {
            X = a.X - b.X;
            Y = a.Y - b.Y;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 Multiply(Vector2 v)
        {
            X *= v.X;
            Y *= v.Y;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 MultiplyScalar(double scalar)
        {
            X *= scalar;
            Y *= scalar;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 Divide(Vector2 v)
        {
            X /= v.X;
            Y /= v.Y;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 DivideScalar(double scalar)
        {
            return MultiplyScalar(1 / scalar);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 ApplyMatrix3(Matrix3 m)
        {
            double x = X, y = Y;
            double[] e = m.Elements;
            X = e[0] * x + e[3] * y + e[6];
            Y = e[1] * x + e[4] * y + e[7];
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 Min(Vector2 v)
        {
            X = Math.Min(X, v.X);
            Y = Math.Min(Y, v.Y);
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 Max(Vector2 v)
        {
            X = Math.Max(X, v.X);
            Y = Math.Max(Y, v.Y);
            return this;
        }

        [MethodImpl((MethodImplOptions)768)]
        public double Min()
        {
            return Math.Min(this.X, this.Y);
        }
        [MethodImpl((MethodImplOptions)768)]
        public double Max()
        {
            return Math.Max(this.X, this.Y);
        }
        public Vector2 Clamp(Vector2 min, Vector2 max)
        {
            // assumes min < max, componentwise
            X = Math.Max(min.X, Math.Min(max.X, X));
            Y = Math.Max(min.Y, Math.Min(max.Y, Y));
            return this;
        }
        public Vector2 ClampScalar(double minVal, double maxVal)
        {
            X = Math.Max(minVal, Math.Min(maxVal, X));
            Y = Math.Max(minVal, Math.Min(maxVal, Y));
            return this;
        }
        public Vector2 ClampLength(double min, double max)
        {
            var length = Length();
            return DivideScalar(length == 0 ? 1 : length).MultiplyScalar(Math.Max(min, Math.Min(max, length)));
        }
        public Vector2 Floor()
        {
            X = Math.Floor(X);
            Y = Math.Floor(Y);
            return this;
        }
        public Vector2 Ceil()
        {
            X = Math.Ceiling(X);
            Y = Math.Ceiling(Y);
            return this;
        }
        public Vector2 Round()
        {
            X = Math.Round(X);
            Y = Math.Round(Y);
            return this;
        }
        public Vector2 RoundToZero()
        {
            X = X < 0 ? Math.Ceiling(X) : Math.Floor(X);
            Y = Y < 0 ? Math.Ceiling(Y) : Math.Floor(Y);
            return this;
        }
        public Vector2 Negate()
        {
            X = -X;
            Y = -Y;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public double Dot(Vector2 v)
        {
            return X * v.X + Y * v.Y;
        }
        [MethodImpl((MethodImplOptions)768)]
        public double Cross(Vector2 v)
        {
            return X * v.Y - Y * v.X;
        }
        [MethodImpl((MethodImplOptions)768)]
        public double LengthSq()
        {
            return X * X + Y * Y;
        }
        [MethodImpl((MethodImplOptions)768)]
        public double Length()
        {
            return Math.Sqrt(X * X + Y * Y);
        }
        [MethodImpl((MethodImplOptions)768)]
        public double ManhattanLength()
        {
            return Math.Abs(X) + Math.Abs(Y);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 Normalize()
        {
            var len = Length();
            if (len == 0) return this;
            X /= len;
            Y /= len;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 GetNormalized()
        {
            var len = Length();
            if (len == 0) return this.Clone();
            return new Vector2(this.X / len, this.Y / len);
        }
        [MethodImpl((MethodImplOptions)768)]
        public double Angle()
        {
            // computes the angle in radians with respect to the positive x-axis
            double angle = Math.Atan2(-Y, -X) + MathEx.PI;
            return angle;
        }
        [MethodImpl((MethodImplOptions)768)]
        public double AngleTo(Vector2 v)
        {
            var denominator = Math.Sqrt(LengthSq() * v.LengthSq());
            if (denominator == 0) return MathEx.PI / 2;
            var theta = Dot(v) / denominator;

            // clamp, to handle numerical problems

            return Math.Acos(MathEx.Clamp(theta, -1, 1));

        }
        [MethodImpl((MethodImplOptions)768)]
        public double DistanceTo(Vector2 v)
        {
            return Math.Sqrt(DistanceToSquared(v));
        }
        [MethodImpl((MethodImplOptions)768)]
        public double DistanceToSquared(Vector2 v)
        {
            double dx = X - v.X, dy = Y - v.Y;
            return dx * dx + dy * dy;
        }
        [MethodImpl((MethodImplOptions)768)]
        public double ManhattanDistanceTo(Vector2 v)
        {
            return Math.Abs(X - v.X) + Math.Abs(Y - v.Y);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 SetLength(double length)
        {
            return Normalize().MultiplyScalar(length);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 Lerp(Vector2 v, double alpha)
        {
            X += (v.X - X) * alpha;
            Y += (v.Y - Y) * alpha;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 LerpVectors(Vector2 v1, Vector2 v2, double alpha)
        {
            X = v1.X + (v2.X - v1.X) * alpha;
            Y = v1.Y + (v2.Y - v1.Y) * alpha;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public override bool Equals(Vector v)
        {
            return Equals(v as Vector2);
        }
        [MethodImpl((MethodImplOptions)768)]
        public bool Equals(Vector2 v)
        {
            return (MathEx.IsEqual(X, v.X) && MathEx.IsEqual(Y, v.Y));
        }
        [MethodImpl((MethodImplOptions)768)]
        public override bool Equals(object obj)
        {
            return Equals(obj as Vector2);
        }
        [MethodImpl((MethodImplOptions)768)]
        public bool Equals(IIOArray obj) => Equals(obj as Vector2);
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 FromArray(double[] array, int offset = 0)
        {
            X = array[offset];
            Y = array[offset + 1];
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        IIOArray IIOArray.FromArray(double[] array, int offset)
        {
            return FromArray(array, offset);
        }
        [MethodImpl((MethodImplOptions)768)]
        public double[] ToArray(double[] array = null, int offset = 0)
        {
            if (array == null) array = new double[2];
            array[offset] = X;
            array[offset + 1] = Y;
            return array;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 RotateAround(Vector2 center, double angle)
        {
            return RotateAround(center.X, center.Y, angle);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Vector2 RotateAround(double cx, double cy, double angle)
        {
            double c = Math.Cos(angle), s = Math.Sin(angle);
            double x = X - cx;
            double y = Y - cy;
            X = x * c - y * s + cx;
            Y = x * s + y * c + cy;
            return this;
        }
        public Vector2 Random()
        {
            X = MathEx.Random();
            Y = MathEx.Random();
            return this;
        }

        public IEnumerator GetEnumerator()
        {
            yield return X;
            yield return Y;
        }


        [MethodImpl((MethodImplOptions)768)]
        public Vector3 ToVector3(double z = 0)
        {
            return new Vector3(X, Y, z);
        }

        public override string ToString()
        {
            return $"x: {X}, y: {Y}";
        }


        [MethodImpl((MethodImplOptions)768)]
        public void Set(double? newX, double? newY, double? newData)
        {
            if (newX != null) this.X = newX.Value;
            if (newY != null) this.Y = newY.Value;
            //if (newData != null) this.Data = newData.Value;
        }

        public static Vector2 UnitX => new Vector2(1, 0);
        public static Vector2 UnitY => new Vector2(0, 1);
        public static Vector2 Zero => new Vector2(0, 0);
        public static Vector2 One => new Vector2(1, 1);

        //public override void Copy(Vector2 from)
        //{
        //    this.X = from.X;
        //    this.Y = from.Y;
        //    //this.Data= from.Data;
        //}

        //public Vector2 Clone()
        //{
        //    return new Vector2(this.X, this.Y);
        //}
        public bool Similarity(Vector2 a, double delta = 1)
        {
            if (a == this)
                return true;
            else if (Vector2.Distance(this, a) < delta)
                return true;
            else
                return false;

        }

        [MethodImpl((MethodImplOptions)768)]
        public static double Dot(Vector2 a, Vector2 b)
        {
            return a.X * b.X + a.Y * b.Y;
        }
        [MethodImpl((MethodImplOptions)768)]
        public static double Cross(Vector2 a, Vector2 b)
        {
            return ((a.X * b.Y) - (a.Y * b.X));
        }

        ///// <summary>
        ///// Returns the unsigned angle in degrees between a and b.
        ///// The smaller of the two possible angles between the two vectors is used.
        ///// The result value range: [0, 180]
        ///// </summary>
        //public static double Angle(Vector2 a, Vector2 b)
        //{
        //    return MathEx.RadianToDegree(AngleInRadian(a, b));
        //}


        /// <summary>
        /// Obtains the angle of a line defined by two points.
        /// </summary>
        /// <param name="u">A Vector2.</param>
        /// <param name="v">A Vector2.</param>
        /// <returns>Angle in radians.</returns>
        [MethodImpl((MethodImplOptions)768)]
        public static double GetAngle(Vector2 u, Vector2 v)
        {
            Vector2 dir = v - u;
            return dir.Angle();
        }


        /// <summary>
        /// Returns the unsigned angle in radians between a and b.
        /// The smaller of the two possible angles between the two vectors is used.
        /// The result value range: [0, PI]
        /// </summary>
        public static double AngleInRadian(Vector2 a, Vector2 b)
        {
            double num = a.Length() * b.Length();
            if (num == 0.0)
            {
                return 0.0;
            }
            double num2 = Dot(a, b) / num;
            return Math.Acos(MathEx.Clamp(num2, -1.0, 1.0));
        }

        /// <summary>
        /// Returns the signed acute clockwise angle in degrees between from and to.
        /// The result value range: [-180, 180]
        /// </summary>
        [MethodImpl((MethodImplOptions)768)]
        public static double SignedAngle(Vector2 from, Vector2 to)
        {
            return MathEx.RadianToDegree(SignedAngleInRadian(from, to));
        }

        /// <summary>
        /// Returns the signed acute clockwise angle in radians between from and to.
        /// The result value range: [-PI, PI]
        /// </summary>
        public static double SignedAngleInRadian(Vector2 from, Vector2 to)
        {
            double rad = AngleInRadian(from, to);
            if (Cross(from, to) < 0)
            {
                rad = -rad;
            }
            return rad;
        }
        [MethodImpl((MethodImplOptions)768)]
        public static double Distance(Vector2 a, Vector2 b)
        {
            Vector2 vector = b - a;
            return vector.Length();
        }

        public static double AnglesDifference(double firstAngle, double secondAngle)
        {
            double difference = secondAngle - firstAngle;
            while (difference < -MathEx.PI) difference += MathEx.TwoPI;
            while (difference > MathEx.PI) difference -= MathEx.TwoPI;

            return difference;
        }

        // Evaluates if the points are clockwise.
        [MethodImpl((MethodImplOptions)768)]
        public static bool Clockwise(Vector2 p1, Vector2 p2, Vector2 p3)
        {
            return ((p2.X - p1.X) * (p3.Y - p1.Y) - (p2.Y - p1.Y) * (p3.X - p1.X)) < 1e-8;
        }
        [MethodImpl((MethodImplOptions)768)]
        public static Vector2 Polar(Vector2 u, double distance, double angle)
        {
            Vector2 dir = new Vector2(Math.Cos(angle), Math.Sin(angle));
            return u + dir * distance;
        }
        [MethodImpl((MethodImplOptions)768)]
        public static Vector2 Rotate(Vector2 v, double angle)
        {
            return RotateInRadian(v, MathEx.DegreeToRadian(angle));
        }
        [MethodImpl((MethodImplOptions)768)]
        public static Vector2 Rotate(Vector2 point, Vector2 basePoint, double angle)
        {
            return RotateInRadian(point, basePoint, MathEx.DegreeToRadian(angle));
        }
        [MethodImpl((MethodImplOptions)768)]
        public static Vector2 RotateInRadian(Vector2 v, double rad)
        {
            double x = v.X * Math.Cos(rad) - v.Y * Math.Sin(rad);
            double y = v.X * Math.Sin(rad) + v.Y * Math.Cos(rad);
            return new Vector2(x, y);
        }

        /// <summary>
        /// Rotates one point around another TM
        /// </summary>
        /// <param name="pointToRotate">The point to rotate.</param>
        /// <param name="centerPoint">The center point of rotation.</param>
        /// <param name="angleInDegrees">The rotation angle in degrees.</param>
        /// <returns>Rotated point</returns>
        public static Vector2 RotateInRadian(Vector2 pointToRotate, Vector2 centerPoint, double angleInRadians)
        {
            double cosTheta = Math.Cos(angleInRadians);
            double sinTheta = Math.Sin(angleInRadians);
            return new Vector2
            {
                X = (cosTheta * (pointToRotate.X - centerPoint.X) - sinTheta * (pointToRotate.Y - centerPoint.Y) + centerPoint.X),
                Y = (sinTheta * (pointToRotate.X - centerPoint.X) + cosTheta * (pointToRotate.Y - centerPoint.Y) + centerPoint.Y)
            };
        }

        public static Vector2 PointOrthoMode(Vector2 last, Vector2 point, bool ortho)
        {
            if (ortho)
            {
                if (Math.Abs(point.X - last.X) > Math.Abs(point.Y - last.Y))
                    return new Vector2(point.X, last.Y);
                else
                    return new Vector2(last.X, point.Y);
            }
            else
            {
                return point;
            }


        }

        [MethodImpl((MethodImplOptions)768)]
        public static Vector2 operator +(Vector2 a, Vector2 b)
        {
            return new Vector2(a.X + b.X, a.Y + b.Y);
        }

        [MethodImpl((MethodImplOptions)768)]
        public static Vector2 operator -(Vector2 a, Vector2 b)
        {
            return new Vector2(a.X - b.X, a.Y - b.Y);
        }

        [MethodImpl((MethodImplOptions)768)]
        public static Vector2 operator -(Vector2 a)
        {
            return new Vector2(-a.X, -a.Y);
        }

        [MethodImpl((MethodImplOptions)768)]
        public static Vector2 operator *(Vector2 a, double d)
        {
            return new Vector2(a.X * d, a.Y * d);
        }
        [MethodImpl((MethodImplOptions)768)]
        public static Vector2 operator *(double d, Vector2 a)
        {
            return new Vector2(a.X * d, a.Y * d);
        }
        [MethodImpl((MethodImplOptions)768)]
        public static Vector2 operator /(Vector2 a, double d)
        {
            return new Vector2(a.X / d, a.Y / d);
        }
        [MethodImpl((MethodImplOptions)768)]
        public static bool operator ==(Vector2 lhs, Vector2 rhs)
        {
            return Object.Equals(lhs, rhs);
        }
        [MethodImpl((MethodImplOptions)768)]
        public static bool operator !=(Vector2 lhs, Vector2 rhs)
        {
            return !Object.Equals(lhs, rhs);
        }
        [MethodImpl((MethodImplOptions)768)]
        public static Vector2 operator *(Vector2 left, Vector2 right)
        {
            return new Vector2(
                left.X * right.X,
                left.Y * right.Y
            );
        }
        public Vector2 Mirror(Vector2 axisStart, Vector2 axisDir)
        {
            var tmpVec = (this - axisStart);
            var prjLen = tmpVec.Dot(axisDir);
            var prjPoint = tmpVec.Copy(axisStart).AddScaledVector(axisDir, prjLen);
            tmpVec.SubVectors(prjPoint, this);
            this.Add(tmpVec.MultiplyScalar(2));
            return this;
        }
        // dimension logic for projections
        public int MaxDim()
        {
            return (this.X >= this.Y) ? 0 : 1;
        }
        public int MinDim()
        {
            return (this.X <= this.Y) ? 0 : 1;
        }
        public double ProjectDim(int dim)
        {
            return (dim == 0) ? this.Y : this.X;
        }


    }
}